| Note: This tutorial assumes that you have completed the previous tutorials: ROS tutorials. |
 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Using the estimated world state
Description: This tutorial explains how to interpret and access the world model output by means of a minimal example.Tutorial Level: INTERMEDIATE
Goal
This tutorial will show how the world model can be fed with evidence.
Approach
Wire generates a world state estimate represemted by the message type wire_msgs/WorldState. Converting the world state to other formats can easily be done using problib in at most a few lines of code. A detailed example present in the source folder of the wire_tutorials package shows how this can be done.
1 #include "ros/ros.h"
2 #include "wire_msgs/ObjectState.h"
3 #include "wire_msgs/WorldState.h"
4
5 #include "problib/conversions.h"
6
7 #include <vector>
8
9 using namespace std;
10
11
12 // Function that extract the Gaussian with the highest weight from a mixture of Gaussians and/or a uniform distribution
13 const pbl::Gaussian* getBestGaussianFromMixture(const pbl::Mixture& mix, double min_weight = 0) {
14 const pbl::Gaussian* G_best = 0;
15 double w_best = min_weight;
16
17 // Iterate over all components
18 for(int i = 0; i < mix.components(); ++i) {
19 const pbl::PDF& pdf = mix.getComponent(i);
20 const pbl::Gaussian* G = pbl::PDFtoGaussian(pdf);
21 double w = mix.getWeight(i);
22
23 // Get Gaussian with the highest associated weight
24 if (G && w > w_best) {
25 G_best = G;
26 w_best = w;
27 }
28 }
29 return G_best;
30 }
31
32
33
34 void worldStateCallback(const wire_msgs::WorldState::ConstPtr& msg) {
35
36 ROS_INFO("Received world state with %d objects", msg->objects.size());
37
38
39 // Iterate over world model objects
40 for (vector<wire_msgs::ObjectState>::const_iterator it_obj = msg->objects.begin(); it_obj != msg->objects.end(); ++it_obj) {
41
42 // Get object from the object array
43 wire_msgs::ObjectState obj = *it_obj;
44 ROS_INFO(" Object:");
45
46 // Iterate over all properties
47 for (vector<wire_msgs::Property>::const_iterator it_prop = obj.properties.begin(); it_prop != obj.properties.end(); ++it_prop) {
48
49 // Get the pdf of the current property
50 pbl::PDF* pdf = pbl::msgToPDF(it_prop->pdf);
51
52 if (pdf) {
53
54 //// Check the attribute
55
56 // Object class
57 if (it_prop->attribute == "class_label") {
58 string label = "";
59 pdf->getExpectedValue(label);
60
61 // Get probability
62 const pbl::PMF* pmf = pbl::PDFtoPMF(*pdf);
63 double p = pmf->getProbability(label);
64 ROS_INFO(" - class %s with probability %f", label.c_str(), p);
65 }
66 // Position
67 else if (it_prop->attribute == "position") {
68 const pbl::Mixture* pos_pdf = pbl::PDFtoMixture(*pdf);
69 if (pos_pdf) {
70 const pbl::Gaussian* pos_gauss = getBestGaussianFromMixture(*pos_pdf);
71 if (pos_gauss) {
72 const pbl::Vector& pos = pos_gauss->getMean();
73 ROS_INFO(" - position: (%f,%f,%f)", pos(0), pos(1), pos(2));
74 ROS_INFO(" - diagonal position cov: (%f,%f,%f)",
75 pos_gauss->getCovariance()(0, 0), pos_gauss->getCovariance()(1, 1), pos_gauss->getCovariance()(2, 2));
76 }
77 } else {
78 ROS_INFO(" - position: object position unknown (uniform distribution)");
79 }
80 }
81 // Orientation
82 else if (it_prop->attribute == "orientation") {
83 const pbl::Mixture* orientation_pdf = pbl::PDFtoMixture(*pdf);
84 if (orientation_pdf) {
85 const pbl::Gaussian* orientation_gauss = getBestGaussianFromMixture(*orientation_pdf);
86 if (orientation_gauss) {
87 const pbl::Vector& ori = orientation_gauss->getMean();
88 ROS_INFO(" - orientation: (%f,%f,%f,%f)", ori(0), ori(1), ori(2), ori(3));
89 }
90 } else {
91 ROS_INFO(" - orientation: object orientation unknown (uniform distribution)");
92 }
93 }
94 // Color
95 else if (it_prop->attribute == "color") {
96 string color = "";
97 pdf->getExpectedValue(color);
98 ROS_INFO(" - color: %s", color.c_str());
99 }
100
101 delete pdf;
102 }
103
104 }
105
106 }
107
108 }
109
110 int main(int argc, char **argv) {
111 ros::init(argc, argv, "get_world_state");
112 ros::NodeHandle nh;
113
114 // Subscribe to the world model
115 ros::Subscriber sub_obj = nh.subscribe("/world_state", 1000, &worldStateCallback);
116
117 ros::spin();
118 return 0;
119 }
The code explained
Include the appropriate header files.
12 // Function that extract the Gaussian with the highest weight from a mixture of Gaussians and/or a uniform distribution
13 const pbl::Gaussian* getBestGaussianFromMixture(const pbl::Mixture& mix, double min_weight = 0) {
14 const pbl::Gaussian* G_best = 0;
15 double w_best = min_weight;
16
17 // Iterate over all components
18 for(int i = 0; i < mix.components(); ++i) {
19 const pbl::PDF& pdf = mix.getComponent(i);
20 const pbl::Gaussian* G = pbl::PDFtoGaussian(pdf);
21 double w = mix.getWeight(i);
22
23 // Get Gaussian with the highest associated weight
24 if (G && w > w_best) {
25 G_best = G;
26 w_best = w;
27 }
28 }
29 return G_best;
30 }
This function gets a mixture of densities as an input and returns the Gaussian distribution associated with the highest weight (both defined in problib). Mixtures are commonly provided by the world model and a mixture can, besides Gaussian distributions, contain a uniform distribution that represents 'object is lost' (we know it must be somewhere, we just do not know where). If the mixture only contains a uniform distribution, G_best will be 0 and the object indeed is lost. This may happen as a result of a high number of propagation steps without intermediate updates combined with a Kalman filter motion model that is associated with a significant amount of uncertainty (process model noise covariance matrix).
Iterate over all objects in the world state estimate and get the current object.
Iterate over all properties associated with this object and get the probability density function representing the current property. This probability density function can either be continuous or discrete.
52 if (pdf) {
Only proceed if a valid probability density function is obtained.
57 if (it_prop->attribute == "class_label") {
If the current property represents the discrete attribute 'class_label'.
Get the most probable value from the probability mass function representing the possible class labels.
Get the probability associated with the most probable class label in the probability mass function representing the class label and print the result.
67 else if (it_prop->attribute == "position") {
If the current property represents the (continuous) attribute position.
68 const pbl::Mixture* pos_pdf = pbl::PDFtoMixture(*pdf);
Get the mixture representing the current position.
70 const pbl::Gaussian* pos_gauss = getBestGaussianFromMixture(*pos_pdf);
Get the most probable Gaussian distribution from the mixture.
If a Gaussian distribution is found, get the mean and the covariance matrix and print the result.
If no Gaussian can be extracted from the mixture, the object is lost and the position is represented by a uniform distribution.
81 // Orientation
82 else if (it_prop->attribute == "orientation") {
83 const pbl::Mixture* orientation_pdf = pbl::PDFtoMixture(*pdf);
84 if (orientation_pdf) {
85 const pbl::Gaussian* orientation_gauss = getBestGaussianFromMixture(*orientation_pdf);
86 if (orientation_gauss) {
87 const pbl::Vector& ori = orientation_gauss->getMean();
88 ROS_INFO(" - orientation: (%f,%f,%f,%f)", ori(0), ori(1), ori(2), ori(3));
89 }
90 } else {
91 ROS_INFO(" - orientation: object orientation unknown (uniform distribution)");
92 }
93 }
If the current property is the (continuous) property orientation, get the mean and covariance matrix of the most probable orientation in the mixture and print the result.
If the current property is the discrete property color, get the most probable value and print the result.
Reproducing the result
Make sure that you have downloaded and compiled the wire packages:
$ git clone https://github.com/tue-robotics/wire.git $ catkin_make
Start a ros core
$ roscore
and launch the wire_core:
$ roslaunch wire_core start.launch
Now start the node created in this tutorial:
$ rosrun wire_tutorials generate_evidence
which creates dummy detections and start the node explained above:
$ rosrun wire_tutorials get_world_state
The following output should appear:
[ INFO] [1359389964.402146962]: Received world state with 0 objects [ INFO] [1359389964.452492126]: Received world state with 1 objects [ INFO] [1359389964.452530330]: Object: [ INFO] [1359389964.452567764]: - position: object position unknown (uniform distribution) [ INFO] [1359389964.452616793]: - color: red [ INFO] [1359389964.452654996]: - class mug with probability 0.998148 [ INFO] [1359389964.502084040]: Received world state with 1 objects [ INFO] [1359389964.502122104]: Object: [ INFO] [1359389964.502146060]: - position: object position unknown (uniform distribution) [ INFO] [1359389964.502168548]: - color: red [ INFO] [1359389964.502193342]: - class mug with probability 0.998148
